#include "TaskObstacle.hpp"
#include "../../Util/BridgeIOConfig.hpp"
#include "cybertron/glm/quat.hpp"
CTaskObstacle::CTaskObstacle() {
	memset(&mLastBridgeObstacleInfo,0,sizeof(SimOne_Data_Obstacle));
	mLastBridgeObstacleInfo.obstacleSize = 0;
	mCommandID = Bridge::EHotAreaObstacleData;
}
CTaskObstacle::~CTaskObstacle() {

}
void CTaskObstacle::HotAreaEnter() {
	memset(&mLastBridgeObstacleInfo, 0, sizeof(SimOne_Data_Obstacle));
	mLastObstacleMap.clear();
}
void CTaskObstacle::HotAreaLeft(int frame, int ms, int msModNs, TrafficActorHotArea* pHotArea) {

}
void CTaskObstacle::ActorToDo(int hostVehicleId, int frame, int ms, int msModNs, TrafficActorHotArea* pHotArea, TrafficActor* pActor, int index) {
		auto pos = pActor->getWorldPosition();
		auto delta = pos - pHotArea->getPosition();
		////apollo only handles 2d map, but cybertron has 3d map
		////for the z diff > 5m obstacles, we won't send to apollo
		//if (fabs(delta.z) > 5.0f)
		//	continue;

		if (mLastBridgeObstacleInfo.obstacleSize >= SOSM_OBSTACLE_SIZE_MAX)
		{
			return;
		}
		std::string hostVehicle = std::to_string(hostVehicleId);
		auto rot = pActor->getWorldRotation();
		auto size = pActor->getObjectSpaceSize();
		//logInfo("frame:%d obstacleSize ====>%d", frame, mLastBridgeObstacleInfo.obstacleSize);
		SimOne_Data_Obstacle_Entry &obs = mLastBridgeObstacleInfo.obstacle[mLastBridgeObstacleInfo.obstacleSize];
		obs.id = SensorConfigUtil::getHashValue(pActor->getRuntimeId());
		obs.viewId = pActor->getRuntimeId();

		float speed = 0.0f;
		switch (pActor->getActorType()) {
		case HotArea::EActorType_TrafficVehicle:
		{
			HotArea::TrafficVehiclePrivateData data;
			if (pActor->getCustomProperties(data))
			{
				speed = data.speed();
				obs.type = (ESimOne_Obstacle_Type)data.semantictype();
			}
			break;
		}
		case HotArea::EActorType_Pedestrian:
		{
			HotArea::PedestrianPrivateData data;
			if (pActor->getCustomProperties(data))
			{
				speed = data.speed();
				obs.type = (ESimOne_Obstacle_Type)data.semantictype();
			}
			break;
		}
		case HotArea::EActorType_Bicycle:
		{
			HotArea::TwoWheelVehiclePrivateData data;
			if (pActor->getCustomProperties(data))
			{
				speed = data.speed();
				obs.type = (ESimOne_Obstacle_Type)data.semantictype();
			}
			break;
		}
		case HotArea::EActorType_StaticObject:
		{
			HotArea::StaticObjectPrivateData data;
			if (pActor->getCustomProperties(data))
			{
				speed = 0;
				obs.type = (ESimOne_Obstacle_Type)data.semantictype();
			}
			break;
		}
		case HotArea::EActorType_MainVehicle:
		{
			obs.type = ESimOne_Obstacle_Type_Car;
			HotArea::MainVehiclePrivateData data;
			if (pActor->getCustomProperties(data))
			{
				speed = data.speed();
			}
			break;
		}
		default:
			obs.type = ESimOne_Obstacle_Type_Unknown;
		}
		obs.theta = rot.z;
		static const float ueActorOffset = 0.88f; // actor has default offset 0.88
		obs.posX = pos.x;
		obs.posY = pos.y;
		obs.posZ = pos.z - ueActorOffset + size.z / 2;
		obs.oriX = rot.x;
		obs.oriY = rot.y;
		obs.oriZ = rot.z;
		quat q(rot);
		vec3 velDir = q * (vec3(1, 0, 0));
		velDir *= speed;
		obs.velX = velDir[0];
		obs.velY = velDir[1];
		obs.velZ = velDir[2];
		obs.length = size.x;
		obs.width = size.y;
		obs.height = size.z;
		mLastBridgeObstacleInfo.obstacleSize++;
		mLastObstacleMap[std::to_string(hostVehicleId)] = mLastBridgeObstacleInfo;
}

char* CTaskObstacle::GetResult(std::string hostVehicleId) {
	for (auto item = mLastObstacleMap.begin(); item != mLastObstacleMap.end(); item++) {
		if (item->first == hostVehicleId) {
			return (char*)&item->second;
		}
	}
	return nullptr;
	//return (char*)&mLastBridgeObstacleInfo;
}

int CTaskObstacle::GetHotAreaDataLength(std::string hostVehicleId) {
	return sizeof(mLastBridgeObstacleInfo);
}

void CTaskObstacle::Clear() {
	mResultMessage.clear();
	mLastBridgeObstacleInfo.obstacleSize = 0;
}
